#include "katana_utils.h"
#include "MatrixUtils.h"
#include "IK.h"
#include "offset.h"

IKModel * ik;
CLMBase * katana;

int main(void)
{
  ik = new IKModel("katanaG.xml");
  katana = initKatanaG();
  katana->switchRobotOff();
  Vector position,joints_angles,rposition;
  char c = 0;
  while(c!=' ')
    {
      getAnglesValues(katana,joints_angles);
      joints_angles = offsetAnglesG(joints_angles);
      joints_angles.Print();
      position = ik->direct(joints_angles);
      ik->jacobian(joints_angles).Print();
      position.Print();
      c = getch();
    }
 }
